#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <Eigen/Core>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
typedef pcl::PointXYZRGB PointT;
using namespace Eigen;
int main ()
{
  
    pcl::PCDReader reader;
    pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>), cloud_f (new pcl::PointCloud<PointT>);
    pcl::io::loadPCDFile ("/home/lfz_5/文档/robot/Files/PointCloud/2.pcd", *cloud);
    boost::shared_ptr<pcl::visualization::PCLVisualizer> vis(new pcl::visualization::PCLVisualizer("3D Viewer"));
  
    pcl::VoxelGrid<PointT> vg;
    pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
    vg.setInputCloud (cloud);
    vg.setLeafSize (0.006f, 0.006f, 0.006f);
    vg.filter (*cloud);

    pcl::PassThrough<PointT> pass;
    pass.setInputCloud (cloud);
    pass.setFilterFieldName ("z");
    //1,5
    pass.setFilterLimits (0.74, 1.5);
    //2
    pass.setFilterLimits (0.55, 0.6);
    // pass.setFilterLimits (0.6, 0.8);//3,4
    // pass.setFilterLimitsNegative (true);
    pass.filter (*cloud);
    *cloud_filtered=*cloud;
    vis->addPointCloud<PointT> (cloud_filtered,"cloud");
    pcl::PCDWriter writer;
  
 
    pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
    tree->setInputCloud (cloud);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointT> ec;
    ec.setClusterTolerance (0.02); // 2cm
    ec.setMinClusterSize (100);
    ec.setMaxClusterSize (25000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);
    double minz=1e5,minx=1e5;
    int j = 0;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
        pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
            cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
        cloud_cluster->width = cloud_cluster->points.size ();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;
        if(cloud_cluster->points.size ()<500) continue;
        Vector4f c;
        pcl::compute3DCentroid(*cloud_cluster,c);
        std::cout<<c[0]<<" "<<c[1]<<" "<<c[2]<<std::endl;
        if(minz>c[2]) minz=c[2];
        else if(minz==c[2]) 
        {
            if(minx>c[0]) minx=c[0];
            else continue;
        }
        else continue;
        std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
        std::stringstream ss;
        // ss << "target" <<j<< ".pcd";
        ss << "target" << ".pcd";
        writer.write<PointT> (ss.str (), *cloud_cluster, false); //*
        std::cout<<"write ok"<<std::endl;
        j++;
    }
    // vis->addPointCloud<PointT> (cloud_filtered,"cloud");
    vis->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "cloud");
    // vis->addPointCloudNormals<pcl::PointT,pcl::Normal> (cloud,cloud_normals,5,0.02,"normals");
    vis->addCoordinateSystem(1);
    while (!vis->wasStopped())
	{
		vis->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
    return (0);
}